#include "robotdef.h"
#include "user_lib.h"
#include <math.h>

#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f)     // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f)   // 半轮距
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长

#define LF_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RF_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE - CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define LB_CENTER ((HALF_TRACK_WIDTH + CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)
#define RB_CENTER ((HALF_TRACK_WIDTH - CENTER_GIMBAL_OFFSET_X + HALF_WHEEL_BASE + CENTER_GIMBAL_OFFSET_Y) * DEGREE_2_RAD)

#define MAX_WHEEL_RPM 8000

//内部函数声明
static void MecanumCalculate(float chassis_vx, float chassis_vy, float chassis_wz,float_t *wheel_ops);
static void OmniCalculate(float chassis_vx, float chassis_vy, float chassis_wz,float_t *wheel_ops);
static void HelmCalculate(float chassis_vx, float chassis_vy, float chassis_wz,float_t *wheel_ops);


void ChassisCalculate(float chassis_vx, float chassis_vy, float chassis_wz,float *wheel_ops)
{
#if CHASSIS_TYPE == 1
    MecanumCalculate(chassis_vx,chassis_vy,chassis_wz,wheel_ops);
#elif CHASSIS_TYPE == 2
    OmniCalculate(chassis_vx,chassis_vy, chassis_wz,wheel_ops);
#elif CHASSIS_TYPE == 3
    HelmCalculate(chassis_vx,chassis_vy,chassis_wz,wheel_ops);
#else
    #error "chassis type error"
#endif 

}
static void OmniCalculate(float chassis_vx, float chassis_vy, float chassis_wz, float_t *wheel_ops)
{
    float max = 0.0f;

    // 计算每个轮子的线速度 (m/s)
    float vt_lf = chassis_vx - chassis_vy - chassis_wz * LF_CENTER;
    float vt_rf = -chassis_vx - chassis_vy - chassis_wz * RF_CENTER;
    float vt_lb = -chassis_vx + chassis_vy - chassis_wz * LB_CENTER;
    float vt_rb = chassis_vx + chassis_vy - chassis_wz * RB_CENTER;

    // 将线速度转换为电机转速 (RPM)
    wheel_ops[0] = (vt_lf / PERIMETER_WHEEL) * 60.0f * REDUCTION_RATIO_WHEEL;
    wheel_ops[1] = (vt_rf / PERIMETER_WHEEL) * 60.0f * REDUCTION_RATIO_WHEEL;
    wheel_ops[2] = (vt_lb / PERIMETER_WHEEL) * 60.0f * REDUCTION_RATIO_WHEEL;
    wheel_ops[3] = (vt_rb / PERIMETER_WHEEL) * 60.0f * REDUCTION_RATIO_WHEEL;

    // 找到 wheel_ops 中的最大绝对值
    for (uint8_t i = 0; i < 4; i++)
    {
        if (abs(wheel_ops[i]) > max)
        {
            max = abs(wheel_ops[i]);
        }
    }

    // 如果最大值超过 MAX_WHEEL_RPM，则按比例缩放
    if (max > MAX_WHEEL_RPM)
    {
        float rate = (MAX_WHEEL_RPM) / max;
        for (uint8_t i = 0; i < 4; i++)
        {
            wheel_ops[i] *= rate;
        }
    }
}